/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef GAVANT_H
#define GAVANT_H

#include "Object.h"
#include "Shapes.h"
#include "Sick.h"
#define GIMI_CLIENT_API 20000
#include "gimi.h"
#include "../MaCI/interfaces/SpeedCtrl/SpeedCtrlServer.hpp"
#include "../MaCI/interfaces/SpeedCtrl/SpeedCtrlTypes.hpp"
#include "JointGroupCtrl/JointGroupCtrlServer.hpp"
#include "JointGroupCtrl/JointGroupCtrlTypes.hpp"

#include "SandSim/Sand.hpp"

//#define MANIPULATOR_POSITION_CONTROL

/*
Class defining simplified ode model for Gavant-machine.

dimensions for model are given in Gavant_settings.h
*/

class CGavant : public CObject
{
  public:
    CGavant(CWorld* World, float x, float y, float heading, std::string name);
    ~CGavant();

    void Activate();

    void Update(float StepSize);

    void OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2);

    void Draw();
	// driving speed
    void SetSpeed(float speed, float angularSpeed, float acceleration);

/*	// for old speed control:
    void SetBoomRotSpeed(float speed, float angularSpeed, float acceleration);
    void SetBoomPrismSpeed(float speed, float angularSpeed, float acceleration);
    void SetScoopRotSpeed(float speed, float angularSpeed, float acceleration);
*/
	// for JointGroupControl:
	// Speed Control:
    void SetTelescopeJointSpeed(float speed);
    void SetBoomJointSpeed(float speed);
    void SetBucketJointSpeed(float speed);

	// Position Control:
    float GetTelescopeJointPosition();
    float GetBoomJointPosition();
    float GetScoopJointPosition();

    void ControlBoomJoint();
    void ControlTelescopeJoint();
    void ControlScoopJoint();

  private:
    std::string name;

    TMaterial WheelMaterial, BodyMaterial, RearBodyMaterial, BoomMaterial, ScoopMaterial, EdgeMaterial;

    dBodyID Chassis, Upper_Boom, Scoop;

    CBox *Rear_Chassis;
    CBox *Central_Chassis;
    CBox *Upper_Chassis;

    CBox *Bottom_Upper_Boom;
    CBox *Top_Upper_Boom;
    CBox *Lower_Boom;

    CBox *Scoop_Back;
    CBox *Scoop_Bottom;
    CBox *Cutting_Edge;

    CBox *Test_Box;

    CTrimesh *Left_Scoop;
    CTrimesh *Right_Scoop;

    CCylinder *RightFrontWheel, *LeftFrontWheel;
    CCylinder *RightRearWheel, *LeftRearWheel;

    dJointID BoomRot, ScoopRot;
    dJointID BoomPrism;
    dJointID RightFrontMotor, LeftFrontMotor;
    dJointID RightRearMotor, LeftRearMotor;

    CLaserScanner *laserScanner;
    
    gimi::GIMI gimi;
    MaCI::MaCICtrl::CMaCICtrlServer mcs;
    MaCI::SpeedCtrl::CSpeedCtrlServer SpeedCtrlServer;
    MaCI::SpeedCtrl::CSpeedCtrlData SpeedCtrlData;
/*    MaCI::SpeedCtrl::CSpeedCtrlServer SpeedCtrlServer1;
    MaCI::SpeedCtrl::CSpeedCtrlServer SpeedCtrlServer2;
    MaCI::SpeedCtrl::CSpeedCtrlServer SpeedCtrlServer3;*/
    MaCI::JointGroupCtrl::CJointGroupCtrlServer jointServer;

    CSand *iSand;

    gim::binbag::CBinBag binbag;

    float desiredBoomAngle;
    float desiredTelescopePosition;
    float desiredScoopAngle;
    float currentBoomAngle;
    float currentTelescopePosition;
    float currentScoopAngle;
    float boomJointError;
    float telescopeJointError;
    float scoopJointError;
    //void setJointAngles();//float boomAngle, float bucketAngle);*/

    void connectGIMI();
};

#endif //GAVANT_H
